#include "kalman_fusemag.h"

/* Function Definitions */
/*
 * FUSEMAG Correct state estimates using magnetometer
 *    [RES, RESCOV] = fusemag(FUSE, MAG, RMAG) fuses magnetometer
 *    data to correct the state estimate. The inputs are:
 *
 *        FUSE      - insfilterMARG object
 *        MAG       - 1-by-3 vector of magnetic field measurements
 *                    in uT.
 *        RMAG      - scalar, 1-by-3, or 3-by-3 covariance of the
 *                    magnetometer measurement error in uT^2
 *
 *    The outputs are:
 *        RES             - 1-by-3 residuals in uT
 *        RESCOV          - 3-by-3 residual covariance
 *
 *    See also residualmag.
 *
 * Arguments    : const kalman_data obj
 *                const double mag[3]
 *                double Rmag
 *                double State[22]
 *                double StateCovariance[484]
 *                double res[3]
 *                double resCov[9]
 * Return Type  : void
 */
void kalman_fusemag( kalman_data *obj,  double mag[3], double Rmag,
                    double res[3], double resCov[9])
{
//  kalman_data c_obj[10];
  double P[484];
  double b_W[484];
  double W[66];
  double b_dhdx[66];
  double dhdx[66];
  double xk[22];
  double Rmagmat[9];
  double b_obj[3];
  double a21;
  double b_dhdx_tmp;
  double c_dhdx_tmp;
  double d_dhdx_tmp;
  double dhdx_tmp;
  double dhdx_tmp_tmp;
  double e_dhdx_tmp;
  double f_dhdx_tmp;
  double g_dhdx_tmp;
  double h_dhdx_tmp;
  double i_dhdx_tmp;
  double j_dhdx_tmp;
  double k_dhdx_tmp;
  double l_dhdx_tmp;
  double m_dhdx_tmp;
  double maxval;
  double n_dhdx_tmp;
  double o_dhdx_tmp;
  double p_dhdx_tmp;
  double q_dhdx_tmp;
  double qparts_idx_1;
  double qparts_idx_2;
  int W_tmp;
  int b_W_tmp;
  int i;
  int k;
  int r1;
  int r2;
  int r3;
  int rtemp;
  /*  kalman_validateattributes(mag, {'double', 'single'}, ... */
  /*      {'2d', 'ncols', 3, 'nrows', 1, 'nonempty', 'real'}, '', ... */
  /*      'magneticField'); */
  /* VALIDATEEXPANDNOISE validate measurement noise input and expand */
  /* to matrix */
  /*    The optional numstr argument is a string (or char) of the */
  /*    num argument. This avoids doing a sprintf or string() */
  /*    conversion in the loop. Must be optional because correct() */
  /*    cannot know this value at design time. */
  /* VALIDATEANDEXPANDNOISE validate measurement noise input and expand */
  /* to matrix */
  /*    The optional numstr argument is a string (or char) of the */
  /*    num argument. This avoids doing a sprintf or string() */
  /*    conversion in the loop. Must be optional because correct() */
  /*    cannot know this value at design time. */
  /*  Figure out Rout with an if-elseif-else statement, not a */
  /*  switch statement, otherwise codegen throws a warning if */
  /*  num==1 and we have redundant branches. */
  memset(&Rmagmat[0], 0, 9U * sizeof(double));
  /*  Basic EKF correct  */
  /* MAGMEASFCN Measurement function Hmag(x) for state vector x */
  /*    3 measurements from magnetometer */
  /*    [magx, magy, magz]; */
  /* MAGMEASJACOBIANFCN Compute the jacobian dHmag/dx of measurement function
   * Hmag(x) */
  dhdx_tmp = 2.0 * obj->State[18] * obj->State[2];
  b_dhdx_tmp = 2.0 * obj->State[17] * obj->State[3];
  c_dhdx_tmp = 2.0 * obj->State[16] * obj->State[0];
  d_dhdx_tmp = (b_dhdx_tmp - dhdx_tmp) + c_dhdx_tmp;
  dhdx[0] = d_dhdx_tmp;
  maxval = (2.0 * obj->State[18] * obj->State[3] +
            2.0 * obj->State[17] * obj->State[2]) +
           2.0 * obj->State[16] * obj->State[1];
  dhdx[3] = maxval;
  a21 = 2.0 * obj->State[18] * obj->State[0];
  e_dhdx_tmp = 2.0 * obj->State[17] * obj->State[1];
  f_dhdx_tmp = 2.0 * obj->State[16] * obj->State[2];
  dhdx[6] = (e_dhdx_tmp - a21) - f_dhdx_tmp;
  qparts_idx_1 = 2.0 * obj->State[16] * obj->State[3];
  qparts_idx_2 = 2.0 * obj->State[17] * obj->State[0];
  dhdx_tmp_tmp = 2.0 * obj->State[18] * obj->State[1];
  g_dhdx_tmp = (dhdx_tmp_tmp + qparts_idx_2) - qparts_idx_1;
  dhdx[9] = g_dhdx_tmp;
  dhdx[12] = 0.0;
  dhdx[15] = 0.0;
  dhdx[18] = 0.0;
  dhdx[21] = 0.0;
  dhdx[24] = 0.0;
  dhdx[27] = 0.0;
  dhdx[30] = 0.0;
  dhdx[33] = 0.0;
  dhdx[36] = 0.0;
  dhdx[39] = 0.0;
  dhdx[42] = 0.0;
  dhdx[45] = 0.0;
  h_dhdx_tmp = obj->State[0] * obj->State[0];
  i_dhdx_tmp = obj->State[1] * obj->State[1];
  j_dhdx_tmp = obj->State[2] * obj->State[2];
  k_dhdx_tmp = obj->State[3] * obj->State[3];
  l_dhdx_tmp = ((h_dhdx_tmp + i_dhdx_tmp) - j_dhdx_tmp) - k_dhdx_tmp;
  dhdx[48] = l_dhdx_tmp;
  m_dhdx_tmp = 2.0 * obj->State[1] * obj->State[2];
  n_dhdx_tmp = 2.0 * obj->State[0] * obj->State[3];
  o_dhdx_tmp = n_dhdx_tmp + m_dhdx_tmp;
  dhdx[51] = o_dhdx_tmp;
  p_dhdx_tmp = 2.0 * obj->State[0] * obj->State[2];
  q_dhdx_tmp = 2.0 * obj->State[1] * obj->State[3];
  dhdx[54] = q_dhdx_tmp - p_dhdx_tmp;
  dhdx[57] = 1.0;
  dhdx[60] = 0.0;
  dhdx[63] = 0.0;
  dhdx[1] = g_dhdx_tmp;
  a21 = (a21 - e_dhdx_tmp) + f_dhdx_tmp;
  dhdx[4] = a21;
  dhdx[7] = maxval;
  dhdx[10] = (dhdx_tmp - b_dhdx_tmp) - c_dhdx_tmp;
  dhdx[13] = 0.0;
  dhdx[16] = 0.0;
  dhdx[19] = 0.0;
  dhdx[22] = 0.0;
  dhdx[25] = 0.0;
  dhdx[28] = 0.0;
  dhdx[31] = 0.0;
  dhdx[34] = 0.0;
  dhdx[37] = 0.0;
  dhdx[40] = 0.0;
  dhdx[43] = 0.0;
  dhdx[46] = 0.0;
  dhdx[49] = m_dhdx_tmp - n_dhdx_tmp;
  dhdx_tmp = h_dhdx_tmp - i_dhdx_tmp;
  b_dhdx_tmp = (dhdx_tmp + j_dhdx_tmp) - k_dhdx_tmp;
  dhdx[52] = b_dhdx_tmp;
  c_dhdx_tmp = 2.0 * obj->State[2] * obj->State[3];
  e_dhdx_tmp = 2.0 * obj->State[0] * obj->State[1];
  f_dhdx_tmp = e_dhdx_tmp + c_dhdx_tmp;
  dhdx[55] = f_dhdx_tmp;
  dhdx[58] = 0.0;
  dhdx[61] = 1.0;
  dhdx[64] = 0.0;
  dhdx[2] = a21;
  dhdx[5] = (qparts_idx_1 - qparts_idx_2) - dhdx_tmp_tmp;
  dhdx[8] = d_dhdx_tmp;
  dhdx[11] = maxval;
  dhdx[14] = 0.0;
  dhdx[17] = 0.0;
  dhdx[20] = 0.0;
  dhdx[23] = 0.0;
  dhdx[26] = 0.0;
  dhdx[29] = 0.0;
  dhdx[32] = 0.0;
  dhdx[35] = 0.0;
  dhdx[38] = 0.0;
  dhdx[41] = 0.0;
  dhdx[44] = 0.0;
  dhdx[47] = 0.0;
  d_dhdx_tmp = p_dhdx_tmp + q_dhdx_tmp;
  dhdx[50] = d_dhdx_tmp;
  dhdx[53] = c_dhdx_tmp - e_dhdx_tmp;
  dhdx_tmp = (dhdx_tmp - j_dhdx_tmp) + k_dhdx_tmp;
  dhdx[56] = dhdx_tmp;
  dhdx[59] = 0.0;
  dhdx[62] = 0.0;
  dhdx[65] = 1.0;
  /*  [x, P, innov, innovCov] ... */
  /*      = positioning.internal.EKF.equationCorrect( ... */
  /*      x, P, h, H, z, R); */
  for (rtemp = 0; rtemp < 3; rtemp++) {
    Rmagmat[rtemp + 3 * rtemp] = Rmag;
    for (i = 0; i < 22; i++) {
      W_tmp = rtemp + 3 * i;
      W[i + 22 * rtemp] = dhdx[W_tmp];
      a21 = 0.0;
      for (b_W_tmp = 0; b_W_tmp < 22; b_W_tmp++) {
        a21 += dhdx[rtemp + 3 * b_W_tmp] *
               obj->StateCovariance[b_W_tmp + 22 * i];
      }
      b_dhdx[W_tmp] = a21;
    }
  }
  b_obj[0] = ((obj->State[19] + obj->State[16] * l_dhdx_tmp) -
              obj->State[18] * (p_dhdx_tmp - q_dhdx_tmp)) +
             obj->State[17] * o_dhdx_tmp;
  b_obj[1] = ((obj->State[20] + obj->State[17] * b_dhdx_tmp) +
              obj->State[18] * f_dhdx_tmp) -
             obj->State[16] * (n_dhdx_tmp - m_dhdx_tmp);
  b_obj[2] = ((obj->State[21] + obj->State[18] * dhdx_tmp) -
              obj->State[17] * (e_dhdx_tmp - c_dhdx_tmp)) +
             obj->State[16] * d_dhdx_tmp;
  for (i = 0; i < 3; i++) {
    for (b_W_tmp = 0; b_W_tmp < 3; b_W_tmp++) {
      a21 = 0.0;
      for (rtemp = 0; rtemp < 22; rtemp++) {
        a21 += b_dhdx[i + 3 * rtemp] * W[rtemp + 22 * b_W_tmp];
      }
      rtemp = i + 3 * b_W_tmp;
      resCov[rtemp] = a21 + Rmagmat[rtemp];
    }
    res[i] = mag[i] - b_obj[i];
  }
  for (i = 0; i < 22; i++) {
    for (b_W_tmp = 0; b_W_tmp < 3; b_W_tmp++) {
      a21 = 0.0;
      for (rtemp = 0; rtemp < 22; rtemp++) {
        a21 += obj->StateCovariance[i + 22 * rtemp] * W[rtemp + 22 * b_W_tmp];
      }
      b_dhdx[i + 22 * b_W_tmp] = a21;
    }
  }
  memcpy(&Rmagmat[0], &resCov[0], 9U * sizeof(double));
  r1 = 0;
  r2 = 1;
  r3 = 2;
  maxval = fabs(resCov[0]);
  a21 = fabs(resCov[1]);
  if (a21 > maxval) {
    maxval = a21;
    r1 = 1;
    r2 = 0;
  }
  if (fabs(resCov[2]) > maxval) {
    r1 = 2;
    r2 = 1;
    r3 = 0;
  }
  Rmagmat[r2] = resCov[r2] / resCov[r1];
  Rmagmat[r3] /= Rmagmat[r1];
  Rmagmat[r2 + 3] -= Rmagmat[r2] * Rmagmat[r1 + 3];
  Rmagmat[r3 + 3] -= Rmagmat[r3] * Rmagmat[r1 + 3];
  Rmagmat[r2 + 6] -= Rmagmat[r2] * Rmagmat[r1 + 6];
  Rmagmat[r3 + 6] -= Rmagmat[r3] * Rmagmat[r1 + 6];
  if (fabs(Rmagmat[r3 + 3]) > fabs(Rmagmat[r2 + 3])) {
    rtemp = r2;
    r2 = r3;
    r3 = rtemp;
  }
  Rmagmat[r3 + 3] /= Rmagmat[r2 + 3];
  Rmagmat[r3 + 6] -= Rmagmat[r3 + 3] * Rmagmat[r2 + 6];
  for (k = 0; k < 22; k++) {
    W_tmp = k + 22 * r1;
    W[W_tmp] = b_dhdx[k] / Rmagmat[r1];
    rtemp = k + 22 * r2;
    W[rtemp] = b_dhdx[k + 22] - W[W_tmp] * Rmagmat[r1 + 3];
    b_W_tmp = k + 22 * r3;
    W[b_W_tmp] = b_dhdx[k + 44] - W[W_tmp] * Rmagmat[r1 + 6];
    W[rtemp] /= Rmagmat[r2 + 3];
    W[b_W_tmp] -= W[rtemp] * Rmagmat[r2 + 6];
    W[b_W_tmp] /= Rmagmat[r3 + 6];
    W[rtemp] -= W[b_W_tmp] * Rmagmat[r3 + 3];
    W[W_tmp] -= W[b_W_tmp] * Rmagmat[r3];
    W[W_tmp] -= W[rtemp] * Rmagmat[r2];
    a21 = W[k + 22];
    maxval = W[k + 44];
    xk[k] =
        obj->State[k] + ((W[k] * res[0] + a21 * res[1]) + maxval * res[2]);
    for (i = 0; i < 22; i++) {
      b_W[k + 22 * i] = (W[k] * dhdx[3 * i] + a21 * dhdx[3 * i + 1]) +
                        maxval * dhdx[3 * i + 2];
    }
    for (i = 0; i < 22; i++) {
      a21 = 0.0;
      for (b_W_tmp = 0; b_W_tmp < 22; b_W_tmp++) {
        a21 += b_W[k + 22 * b_W_tmp] * obj->StateCovariance[b_W_tmp + 22 * i];
      }
      rtemp = k + 22 * i;
      P[rtemp] = obj->StateCovariance[rtemp] - a21;
    }
  }
  /*  Normalize quaternion and enforce a positive angle of */
  /*  rotation. Avoid constructing a quaternion here. Just do the */
  /*  math inline. */
  /*  Used cached OrientationIdx */
  maxval =
      sqrt(((xk[0] * xk[0] + xk[1] * xk[1]) + xk[2] * xk[2]) + xk[3] * xk[3]);
  a21 = xk[0] / maxval;
  qparts_idx_1 = xk[1] / maxval;
  qparts_idx_2 = xk[2] / maxval;
  maxval = xk[3] / maxval;
  if (a21 < 0.0) {
    xk[0] = -a21;
    xk[1] = -qparts_idx_1;
    xk[2] = -qparts_idx_2;
    xk[3] = -maxval;
  } else {
    xk[0] = a21;
    xk[1] = qparts_idx_1;
    xk[2] = qparts_idx_2;
    xk[3] = maxval;
  }
  /*  Make the innovation a row vector to match the measurement shape */
  
    memcpy(&obj->StateCovariance[0], &P[0], 484U * sizeof(double));
  
    memcpy(&obj->State[0], &xk[0], 22U * sizeof(double));
 
}
